/*****************************************************************************
The init_ardupilot function processes everything we need for an in-air restart
	We will determine later if we are actually on the ground and process a
	ground start in that case.

Also in this function we will dump the log if applicable based on the slide switch
*****************************************************************************/
void init_ardupilot()
{

	#if GPS_PROTOCOL == -1
		Serial.begin(NO_GPS);
	#elif GPS_PROTOCOL == 0
		Serial.begin(NMEA_GPS);
	#elif GPS_PROTOCOL == 1
		Serial.begin(EM406_GPS);
	#elif GPS_PROTOCOL == 2
		Serial.begin(UBLOX_GPS);
	#elif GPS_PROTOCOL == 3
		Serial.begin(ARDU_IMU);
	#elif GPS_PROTOCOL == 4
		Serial.begin(MTK_GPS);
	#elif GPS_PROTOCOL == 5
		Serial.begin(SIM_GPS);
	#endif

	// Enable GPS
	// ----------------
	setGPSMux();
	delay(100);
	//Serial.printf_P(PSTR("\n\nAP 2.8\n"));
	Serial.print("\n\nAP 2.8\n");

 	// ATMEGA ADC
 	// PC0 - ADC0 	- 23 - X sensor
 	// PC1 - ADC1 	- 24 - Y sensor
 	// PC2 - ADC2 	- 25 - Z sensor
 	// PC3 - ADC3 	- 26 - Pressure sensor
 	// PC4 - ADC4 	- 27 -
 	// PC5 - ADC5 	- 28 - Battery Voltage

 	// ATMEGA
	// PORTD
	// p0				// PD0 - RXD  		- Serial RX
	// p1				// PD1 - TXD  		- Serial TX
	//pinMode(2,INPUT);	// PD2 - INT0 		- Rudder in							- INPUT Rudder/Aileron
	//pinMode(3,INPUT);	// PD3 - INT1 		- Elevator in 						- INPUT Elevator
	pinMode(4,INPUT);	// PD4 - XCK/T0 	- MUX pin							- Connected to Pin 2 on ATtiny
	pinMode(5,INPUT);	// PD5 - T0			- Mode pin							- Connected to Pin 6 on ATtiny   - Select on MUX
	pinMode(6,OUTPUT);	// PD6 - T1			- Ground start signaling Pin
	pinMode(7,OUTPUT);	// PD7 - AIN0		- GPS Mux pin
	// PORTB
	//pinMode(8, OUTPUT); // PB0 - AIN1		- Servo throttle					- OUTPUT THROTTLE
	//pinMode(9, OUTPUT);	// PB1 - OC1A		- Elevator PWM out					- Elevator PWM out
	//pinMode(10,OUTPUT);	// PB2 - OC1B		- Rudder PWM out					- Aileron PWM out
	//pinMode(11,INPUT); 	// PB3 - MOSI/OC2	-
	pinMode(12,OUTPUT); // PB4 - MISO		- Blue LED pin  - GPS Lock			- GPS Lock
	//pinMode(13,INPUT); 	// PB5 - SCK		- Yellow LED pin   					- INPUT Throttle

	// PORTC - Analog ports
	// PC0 - Thermopile - x
	// PC1 - Thermopile - y
	// PC2 - Thermopile - z
	// PC3 - Airspeed
	// PC4 - CH4 OUT - Rudder output
	// PC5 - Battery

	// set Analog out 4 to output
	//DDRC |= B00010000;




	digitalWrite(6,HIGH);

	// setup control switch
	// ----------------
	initControlSwitch();

	// load launch settings from EEPROM
	// --------------------------------
	restore_EEPROM();

	// connect to radio
	// ----------------
	init_radio();

	// setup PIDs
	// ----------
	init_pids();

	// print the radio
	// ---------------
	print_radio_in();

	// set the correct flight mode
	// ---------------------------
	reset_control_switch();

	Serial.printf_P(PSTR("freeRAM: %d\n"), freeRAM());

	if(startup_check()){
		Serial.printf_P(PSTR("MSG Startup: Ground\n"));

		startup_ground();

		// Save the settings for in-air restart
		// ------------------------------------
		save_EEPROM_groundstart();

	}else{
		Serial.printf_P(PSTR("MSG Startup: AIR\n"));
		takeoff_complete = true;
		ground_start_count = 0;

		// Load WP from memory
		// -------------------
		load_waypoint();
		startup_air_event();
	}
}

byte startup_check(void){
	if(DEBUG_SUBSYSTEM > 0){
		debug_subsystem();
	}else{
		if ((readSwitch() == 1) && (rc_3.control_in == 0)){
			// we are in manual, no throttle = ground start
			return 1;
		}else{
			// we are not in manual, and we've got throttle
			return 0;
		}
	}
}

//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
void startup_ground(void)
{
	#if USE_AUTO_LAUNCH == 1
		takeoff_complete	= false;			// Flag for using take-off controls
	#else
		takeoff_complete	= true;
	#endif

	read_as_test();

	// Do GPS init
	//g_gps = &GPS;
	Serial.printf_P(PSTR("init GPS\n"));
	g_gps = &g_gps_driver;
	g_gps->init();			// GPS Initialization

	delay(500);
    byte counter 	= 2;
    GPS_enabled 	= false;

    // Read in the GPS
	for (byte counter = 0; ; counter++) {
		g_gps->update();
		if (g_gps->status() != 0){
			GPS_enabled = true;
			break;
		}

		if (counter >= 2) {
			GPS_enabled = false;
			break;
	    }
	}

	Serial.printf_P(PSTR("GPS: %d\n"),GPS_enabled);

	// Output waypoints for confirmation
	// --------------------------------
	print_waypoints();

	//Signal the IMU to perform ground start
	//------------------------
	digitalWrite(6,LOW);

	// Makes the servos wiggle
	// step 1 = 1 wiggle
	// -----------------------
	demo_servos(1);

	// set a default reasonable ir_max val
	// -----------------------------------
	ir_max = 150;

	// read the radio to set trims
	// ---------------------------
	for (byte i = 0; i < 30; i++){
		read_radio();
	}
	trim_control_surfaces();

	#if AIRSPEED_SENSOR == 1
		// initialize airspeed sensor
		// --------------------------
		zero_airspeed();
	#else
		//Serial.println("NO airspeed");
	#endif

	// Number of reads before saving Home position
	// -------------------------------------------
	ground_start_count = 6;

	// Lower signal pin in case of IMU power glitch
	// --------------------------------------------
	digitalWrite(6,HIGH);

	// Makes the servos wiggle - 3 times signals ready to fly
	// -----------------------
	demo_servos(3);

	Serial.printf_P(PSTR("\nMSG Ready to FLY.\n"));
}


void set_mode(byte mode)
{
	if(control_mode == mode){
		// don't switch modes if we are already in the correct mode.
		return;
	}

	#if AUTO_TRIM == 1
		if(control_mode == MANUAL)
			trim_control_surfaces();
	#endif

	control_mode = mode;

	#if USE_AUTO_LAUNCH == 1
		if(control_mode == AUTO && takeoff_complete == false){
			control_mode = TAKEOFF;
		}
	#endif

	switch(control_mode)
	{
		//case MANUAL:
		//	break;

		//case STABILIZE:
		//	break;

		//case FLY_BY_WIRE_A:
		//	break;

		//case FLY_BY_WIRE_B:
		//	break;

		case AUTO:
			// reload the current WP and set control modes;
			load_waypoint();
			break;

		case RTL:
			return_to_launch();
			break;

		case LOITER:
			loiter_at_location();
			break;

		//case TAKEOFF:
		//	break;

		//case LAND:
		//	break;
	}

	// output control mode to the ground station
	send_message(MSG_HEARTBEAT);
}

void set_failsafe(boolean mode)
{
	// only act on changes
	// -------------------
	if(failsafe != mode){

		// store the value so we don't trip the gate twice
		// -----------------------------------------------
		failsafe = mode;

		if (failsafe == false){
			// We've regained radio contact
			// ----------------------------
			failsafe_off_event();

			// Release hardware MUX just in case it's not set
			// ----------------------------------------------
			set_servo_mux(false);

		}else{
			// We've lost radio contact
			// ------------------------
			failsafe_on_event();
		}
	}
}



// This hack is to control the V2 shield so we can read the serial from
// the XBEE radios - which is not implemented yet
void setGPSMux(void)
{
	#if SHIELD_VERSION < 1 || GPS_PROTOCOL == 3 // GPS_PROTOCOL == 3 -> With IMU always go low.
		digitalWrite(7, LOW); //Remove Before Fly Pull Up resistor
    #else
		digitalWrite(7, HIGH); //Remove Before Fly Pull Up resistor
	#endif
}


void setCommandMux(void)
{
	#if SHIELD_VERSION < 1
		digitalWrite(7, HIGH); //Remove Before Fly Pull Up resistor
    #else
		digitalWrite(7, LOW); //Remove Before Fly Pull Up resistor
	#endif
}

void update_GPS_light(void)
{
	// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
	// ---------------------------------------------------------------------
	switch (g_gps->status()){

		case(2):
			digitalWrite(BLUE_LED_PIN, HIGH);  //Turn LED C on when gps has valid fix.
			break;

		case(1):
			if (g_gps->valid_read == true){
				GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
				if (GPS_light){
					digitalWrite(BLUE_LED_PIN, LOW);
				}else{
					digitalWrite(BLUE_LED_PIN, HIGH);
				}
				g_gps->valid_read = false;
			}
			break;

		default:
			digitalWrite(BLUE_LED_PIN, LOW);
			break;
	}
}

/* This function gets the current value of the heap and stack pointers.
* The stack pointer starts at the top of RAM and grows downwards. The heap pointer
* starts just above the static variables etc. and grows upwards. SP should always
* be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6-Feb-2009.
*/
unsigned long freeRAM() {
	uint8_t * heapptr, * stackptr;
	stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
	heapptr = stackptr; // save value of heap pointer
	free(stackptr); // free up the memory again (sets stackptr to 0)
	stackptr = (uint8_t *)(SP); // save value of stack pointer
	return stackptr - heapptr;
}


//***********************************************************************************
//  The following functions are used during startup and are not for telemetry
//***********************************************************************************

void print_radio_in()
{
	Serial.printf_P(PSTR("Radio R_IN 1: %d\t2: %d\t3: %d\t4: %d\n"),
							rc_1.radio_in,
							rc_2.radio_in,
							rc_3.radio_in,
							rc_4.radio_in);
}
void print_control_in()
{
	Serial.printf_P(PSTR("Radio C_IN 1: %d\t2: %d\t3: %d\t4: %d\n"),
							rc_1.control_in,
							rc_2.control_in,
							rc_3.control_in,
							rc_4.control_in);
}

void print_servo_out()
{
	Serial.printf_P(PSTR("Radio S_OUT 1: %d\t2: %d\t3: %d\t4: %d\n"),
							rc_1.servo_out,
							rc_2.servo_out,
							rc_3.servo_out,
							rc_4.servo_out);
}

void print_radio_out()
{
	Serial.printf_P(PSTR("Radio R_OUT 1: %d\t2: %d\t3: %d\t4: %d\n"),
							rc_1.radio_out,
							rc_2.radio_out,
							rc_3.radio_out,
							rc_4.radio_out);
}

void print_waypoints(){
	Serial.printf_P(PSTR("WP Total: %d\n"), wp_total);
	struct Location tmp;

	for (int i = 0; i <= wp_total; i++){
		tmp = get_wp_with_index(i);
		print_wp(&tmp, i);
	}
}

void print_wp(struct Location *cmd, byte index)
{
	Serial.printf_P(PSTR("wp #: %d alt:%ld lat:%ld lng:%ld\n"),
		(int)index,
		cmd->alt,
		cmd->lat,
		cmd->lng);
}



void init_pids(){
	pidNavRoll.kP(NAV_ROLL_P);
	pidNavRoll.kI(NAV_ROLL_I);
	pidNavRoll.kD(NAV_ROLL_D);
	pidNavRoll.imax(NAV_ROLL_IMAX);

	pidNavPitchAirspeed.kP(NAV_PITCH_ASP_P);
	pidNavPitchAirspeed.kI(NAV_PITCH_ASP_I);
	pidNavPitchAirspeed.kD(NAV_PITCH_ASP_D);
	pidNavPitchAirspeed.imax(NAV_PITCH_ASP_IMAX);

	pidServoRoll.kP(SERVO_ROLL_P);
	pidServoRoll.kI(SERVO_ROLL_I);
	pidServoRoll.kD(SERVO_ROLL_D);
	pidServoRoll.imax(SERVO_ROLL_IMAX);

	pidServoPitch.kP(SERVO_PITCH_P);
	pidServoPitch.kI(SERVO_PITCH_I);
	pidServoPitch.kD(SERVO_PITCH_D);
	pidServoPitch.imax(SERVO_PITCH_IMAX);

	pidTeThrottle.kP(THROTTLE_TE_P);
	pidTeThrottle.kI(THROTTLE_TE_I);
	pidTeThrottle.kD(THROTTLE_TE_D);
	pidTeThrottle.imax(THROTTLE_TE_IMAX);

	pidNavPitchAltitude.kP(NAV_PITCH_ALT_P);
	pidNavPitchAltitude.kI(NAV_PITCH_ALT_I);
	pidNavPitchAltitude.kD(NAV_PITCH_ALT_D);
	pidNavPitchAltitude.imax(NAV_PITCH_ALT_IMAX);
}


